#include "nav2_custom_planner/nav2_custom_planner.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_core/exceptions.hpp"
namespace nav2_custom_planner
{

    void CustomPlanner::configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
        std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
    {
        tf_buffer_ = tf;
        node_ = parent.lock();
        name_ = name;
        costmap_ = costmap_ros->getCostmap();
        global_frame_ = costmap_ros->getGlobalFrameID();

        nav2_util::declare_parameter_if_not_declared(node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.1));
        node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_);
    }
    void CustomPlanner::cleanup()
    {
        RCLCPP_INFO(node_->get_logger(), "正在清除类型为CustomPlanner的插件 %s", name_.c_str());
    }
    void CustomPlanner::activate()
    {
        RCLCPP_INFO(node_->get_logger(), "正在激活类型为CustomPlanner的插件 %s", name_.c_str());
    }
    void CustomPlanner::deactivate()
    {
        RCLCPP_INFO(node_->get_logger(), "正在停用类型为CustomPlanner的插件 %s", name_.c_str());
    }
    nav_msgs::msg::Path CustomPlanner::createPlan(
        const geometry_msgs::msg::PoseStamped &start,
        const geometry_msgs::msg::PoseStamped &goal)
    {
        nav_msgs::msg::Path global_path;
        global_path.poses.clear();
        global_path.header.frame_id = global_frame_;
        global_path.header.stamp = node_->now();

        // 检查起始点和目标点是否在全局坐标系中
        if (start.header.frame_id != global_frame_)
        {
            RCLCPP_INFO(node_->get_logger(), "规划器仅接收来自%s坐标系的起始位置", global_frame_.c_str());
            return global_path;
        }
        if (goal.header.frame_id != global_frame_)
        {
            RCLCPP_INFO(node_->get_logger(), "规划器仅接收来自%s坐标系的目标位置", global_frame_.c_str());
            return global_path;
        }

        // 计算当前插值分辨率interpolation_resolution_下的循环次数和步进值
        double total_number_of_loop = std::hypot(goal.pose.position.x - start.pose.position.x, goal.pose.position.y - start.pose.position.y) / interpolation_resolution_;
        double x_increment = (goal.pose.position.x - start.pose.position.x) / total_number_of_loop;
        double y_increment = (goal.pose.position.y - start.pose.position.y) / total_number_of_loop;

        // 生成路径
        for (int i = 0; i < total_number_of_loop; ++i)
        {
            geometry_msgs::msg::PoseStamped pose;
            pose.pose.position.x = start.pose.position.x + x_increment * i;
            pose.pose.position.y = start.pose.position.y + y_increment * i;
            pose.pose.position.z = 0.0;
            pose.header.frame_id = global_frame_;
            pose.header.stamp = node_->now();

            global_path.poses.push_back(pose);
        }

        // 使用costmap检查该路径是否经过障碍物
        for (geometry_msgs::msg::PoseStamped pose : global_path.poses)
        {
            unsigned int mx,my; //将坐标转换为栅格坐标
            if(costmap_->worldToMap(pose.pose.position.x,pose.pose.position.y,mx,my))
            {
                unsigned char cost = costmap_->getCost(mx,my);   //获取对应的栅格的代价值
                // 如果存在致命障碍物则抛出异常
                if(cost == nav2_costmap_2d::LETHAL_OBSTACLE)
                {
                    RCLCPP_WARN( node_->get_logger(),"在(%f,%f)处检测到障碍物，规划失败！",pose.pose.position.x,pose.pose.position.y);
                    throw nav2_core::PlannerException("无法创建目标规划："+std::to_string(goal.pose.position.x)+","+std::to_string(goal.pose.position.y));
                }
            }
        }

        //将目标点作为路径的最后一个点并返回路径
        geometry_msgs::msg::PoseStamped goal_pose = goal;
        goal_pose.header.stamp = node_->now();
        goal_pose.header.frame_id = global_frame_;
        global_path.poses.push_back(goal_pose);
        return global_path;
    }
} // namespase nav2_custom_planner

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_custom_planner::CustomPlanner, nav2_core::GlobalPlanner);
